// This file is part of OpenMeca, an easy software to do mechanical simulation.
//
// Author(s)    :  - Damien ANDRE  <openmeca@gmail.com>
//
// Copyright (C) 2012 Damien ANDRE
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.


// This source file was inspired of the "libGeometrical" from 
// the GranOO workbench : http://www.granoo.org


#ifndef _OpenMeca_Geom_Point_hpp_
#define _OpenMeca_Geom_Point_hpp_

#include <iostream>
#include <string>
#include <vector>

#include "OpenMeca/Geom/SpaceDim.hpp"
#include "OpenMeca/Geom/Coordinate.hpp"
#include "OpenMeca/Geom/Vector.hpp"


#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/function.hpp>


namespace OpenMeca
{
  namespace Geom
  {
    template<SpaceDim N>
    class Point
    {
    
      friend class Vector<N>;
      template<Geom::SpaceDim M> friend std::ostream& operator<<(std::ostream &, const Point<M> &);
      
    public:    
      static std::string GetStrKey(){return std::string("Point" + SpaceDimUtil<N>::GetStrKey());}

    public:
      //CONSTRUCTORS & DESTRUCTORS
      explicit Point(boost::function<const Frame<N>& ()> = &Frame<N>::GetGlobal) ;
      explicit Point(const Point<N> &, boost::function<const Frame<N>& ()>);
      explicit Point(double, double, double, boost::function<const Frame<N>& ()> = &Frame<N>::GetGlobal);
      Point(const Point &);
      virtual ~Point();

      //OPERATORS
      Point & operator=(const Point &);

      const double & operator[](int i) const;
      double& operator[](int i);	

      //ACCESSORS
      SpaceDim GetDimension() const;
      const Frame<N> & GetFrame() const;

      const Coordinate<N> & GetCoordinate() const;
      Coordinate<N> & GetCoordinate();
      
      Vector<N> & GetPositionVector();
      const Vector<N> & GetPositionVector() const;

      //UTILS
      double ComputeDistanceFrom(const Point &) const;
      std::ostream& Write(std::ostream&) const;      

      //FRAME OPERATIONS
      Point ToGlobalFrame() const;
      Point ToLocalFrame(const Frame<N>&) const;
      void  ToGlobalFrame(Point<N>&) const;
      void  ToLocalFrame(const Frame<N>&, Point<N>&) const;

    private:
      // - BOOST SERIALIZATION - //
      friend class boost::serialization::access;
      template<class Archive> void serialize(Archive&, const unsigned int );
    
    private:
      Coordinate<N, Cartesian> coord_;  
    };


    // - BOOST SERIALIZATION - //
    template<SpaceDim N>
    template<class Archive>
    inline void 
    Point<N>::serialize(Archive & ar , const unsigned int ) 
    {
      ar & BOOST_SERIALIZATION_NVP(coord_);
    }   

    // Vorbidden non-specialized methods (sould be specialized)
    //

    
    template<SpaceDim N>
    Point<N>::Point(double x, double y, double z, boost::function<const Frame<N>& ()> f)
    {
      assert(!"Genereic constructour should not be called !!!");
    }


    //
    // Template methods
    //

    // Constructors ...

    template<SpaceDim N> inline
    Point<N>::Point(boost::function<const Frame<N>& ()> f)
      :  coord_(f)
    {
    }

    template<SpaceDim N> inline
    Point<N>::Point(const Point<N> &p)
      :  coord_(p.coord_)
    {
    }


    template<SpaceDim N> inline 
    Point<N>::Point(const Point<N> & p, boost::function<const Frame<N>& ()> f) 
      :  coord_(f)
    {
      if (p.GetFrame()==f())
	{
	  //std::cerr << "point conversion: nothing to do..." << std::endl << std::flush;
	  *this=p;
	}

      else if (p.GetFrame().GetReferenceFrame()==f())//Frame<N>::Global)
	{
	  GetPositionVector()  = p.GetFrame().GetQuaternion().Rotate(p.GetPositionVector());
	  GetPositionVector() += p.GetFrame().GetCenter().GetPositionVector();
	}

      else if (p.GetFrame()==f().GetReferenceFrame())//Frame<N>::Global)
	{
	  GetPositionVector()  = f().GetQuaternion().InverseRotate(p.GetPositionVector());
	  GetPositionVector() -= Vector<N>(f().GetCenter().GetPositionVector(),f);
	}

      else if (f()==Frame<N>::Global)
	{
	  GetPositionVector() = Vector<N>(p.GetPositionVector(),&Frame<N>::GetGlobal);
	  const Frame<N> *refFrame = &p.GetFrame();
	  while(refFrame->GetRank()>1)
	    {
	      GetPositionVector() += Vector<N>(refFrame->GetCenter().GetPositionVector(), &Frame<N>::GetGlobal);
	      refFrame = &refFrame->GetReferenceFrame();
	    }
	  GetPositionVector() += refFrame->GetCenter().GetPositionVector();
	}
      
      else 
	{
	  GetPositionVector() += Vector<N>(p.GetPositionVector(),f);
	  const Frame<N> *refFrame = &p.GetFrame();
	  while(refFrame!=&Frame<N>::Global)
	    {
	      GetPositionVector() += Vector<N>(refFrame->GetCenter().GetPositionVector(), f);
	      refFrame = &refFrame->GetReferenceFrame();
	    }
	  
	  refFrame = &f();
	  while(refFrame!=&Frame<N>::Global)
	    {
	      GetPositionVector() -= Vector<N>(refFrame->GetCenter().GetPositionVector(), f);
	      refFrame = &refFrame->GetReferenceFrame();
	    }
	}
    }

    template<SpaceDim N> inline
    Point<N>::~Point()
    {
    }

    // Accessors

    template<SpaceDim N>
    inline SpaceDim 
    Point<N>::GetDimension() const
    {
      return coord_.dimension;
    }

    template<SpaceDim N>
    inline Vector<N> & 
    Point<N>::GetPositionVector()
    {
      return *reinterpret_cast<Vector<N> *>(this);
    }

    template<SpaceDim N>
    inline const Vector<N> & 
    Point<N>::GetPositionVector() const
    {
      return *reinterpret_cast<const Vector<N> *>(this);
    }

    template<SpaceDim N>
    inline const Coordinate<N> &
    Point<N>::GetCoordinate() const
    {
      return coord_;
    }

    template<SpaceDim N>
    inline Coordinate<N> &
    Point<N>::GetCoordinate()
    {
      return coord_;
    }

 
    template<SpaceDim N>
    inline const Frame<N> & 
    Point<N>::GetFrame() const
    {
      return coord_.GetFrame();
    }
    
    
    // External Operators ...
    

    // Class Operators ...

    template <SpaceDim N> inline 
    const double &
    Point<N>::operator[](int i) const
    { 
      return coord_.c_[i];
    }

    template <SpaceDim N> inline 
    double &
    Point<N>::operator[](int i) 
    { 
      return coord_.c_[i];
    }

    template<SpaceDim N> inline
    std::ostream & 
    Point<N>::Write(std::ostream & out) const
    {
      std::cout.setf(std::ios::scientific, std::ios::floatfield);
      switch (N)
      {
        case _3D : return out << "Point<_3D> (" << coord_[0] << ", " << coord_[1] << ", " << coord_[2] << ")"; break;
        case _2D : return out << "Point<_2D> (" << coord_[0] << ", " << coord_[1]  << ")"; break;
        case _1D : return out << "Point<_1D> (" << coord_[0]  << ")"; break;
        case _0D : return out << "Point<_0D> ()"; break;
        default : assert(0); break;
      }
    }

    template<SpaceDim N> inline
    std::ostream & 
    operator<<(std::ostream& out, const Point<N>& pt)
    { 
      std::cout.setf(std::ios::scientific, std::ios::floatfield);
      switch (N)
      {
        case _3D : return out <<  pt.coord_[0] << "\t" << pt.coord_[1] << "\t"  << pt.coord_[2] <<"\t" ; break;
        case _2D : return out << pt.coord_[0] << "\t"  << pt.coord_[1]  << "\t" ; break;
        case _1D : return out << pt.coord_[0]  << "\t" ; break;
        case _0D : return out << "\t" ; break;
        default : assert(0); break;
      }
    }

    template<SpaceDim N>
    inline Point<N> &
    Point<N>::operator=(const Point<N> & p)
    {
      assert(GetFrame() == p.GetFrame());

      if (this == &p)
	return *this;

      coord_=p.coord_;

      return *this;
    }

    template <SpaceDim N>
    inline Point<N> 
    Point<N>::ToGlobalFrame() const
    {
      assert (GetFrame().GetRank()==1);
      return (GetFrame().GetQuaternion().Rotate(GetPositionVector())+GetFrame().GetCenter().GetPositionVector()).GetPoint();
    }

    template <SpaceDim N>
    inline Point<N>
    Point<N>::ToLocalFrame(const Frame<N>& f) const
    {
      assert (GetFrame().GetRank()==0 && f.GetRank()==1);
      return (f.GetQuaternion().InverseRotate(GetPositionVector()-f.GetCenter().GetPositionVector()).GetPoint());
    }
   
    template <SpaceDim N> inline
    void 
    Point<N>::ToGlobalFrame(Point<N>& p_out) const
    {
      assert (GetFrame().GetRank()==1);
      GetFrame().GetQuaternion().Rotate(GetPositionVector(),p_out.GetPositionVector());
      p_out.coord_+=GetFrame().GetCenter().coord_;
    }
      
    template <SpaceDim N> inline
    void
    Point<N>::ToLocalFrame(const Frame<N>& f, Point<N>& p_out) const
    {
       assert (GetFrame().GetRank()==0 && f.GetRank()==1);
       f.GetQuaternion().InverseRotate(GetPositionVector()-f.GetCenter().GetPositionVector(),p_out.GetPositionVector());
    }
    


    ///////////////////////////////////
    // template specialisation : _3D //
    ///////////////////////////////////

    // Constructors ...

    template<> inline 
    Point<_3D>::Point(double x, double y, double z, boost::function<const Frame<_3D>& ()> f)
      :  coord_(x,y,z,f)
    {
    }

#ifdef CHRONO_ENGINE

    template<> inline
    Point<_3D>::Point (const chrono::ChVector<double> & v, const Frame<_3D> & f)
      :  coord_ (v.x, v.y, v.z, f)
    {
    }

#endif

    // Accessors ...

    // External Operators ...

    // Class Operators ...

    // Usefull Methods

    template<> inline 
    double 
    Point<_3D>::ComputeDistanceFrom(const Point & pt) const
    {
      return sqrt( (coord_.c_[0]-pt.coord_.c_[0])*(coord_.c_[0]-pt.coord_.c_[0])+
		   (coord_.c_[1]-pt.coord_.c_[1])*(coord_.c_[1]-pt.coord_.c_[1])+
		   (coord_.c_[2]-pt.coord_.c_[2])*(coord_.c_[2]-pt.coord_.c_[2]));
    }

  

  } // namespace Geom
}

#endif
